﻿#include <stdio.h>
#define KP 0.2
#define KI 0.4
#define KD 0.05

typedef struct Indicator {
    double ActiveValue; // ��ǰֵ
    double TargetValue; // Ŀ��ֵ
    float OutputMax;   // ��������޷�
    double CurrError;   // e(k)
    double LastError;   // e(k-1)
    double PrevError;   // e(k-2)
} PID_Indicator;

PID_Indicator PID_Initial()
{
    PID_Indicator indicator;
    indicator.ActiveValue = 0;
    indicator.TargetValue = 0;
    indicator.OutputMax = 0;
    indicator.CurrError = 0;
    indicator.LastError = 0;
    indicator.PrevError = 0;
    return indicator;
}

void PID_SetParameters(PID_Indicator* indicator, float ActiveValue, float TargetValue, float OutputMax)
{
    indicator->ActiveValue = ActiveValue;
    indicator->TargetValue = TargetValue;
    indicator->OutputMax = OutputMax;
}

float PID_Calculate(PID_Indicator* indicator)
{
    indicator->PrevError = indicator->LastError;
    indicator->LastError = indicator->CurrError;
    indicator->CurrError = indicator->TargetValue - indicator->ActiveValue;

    double Proportion = KP * (indicator->CurrError - indicator->LastError);
    double Intergral = KI * indicator->CurrError;
    double Differential = KD * (indicator->CurrError - 2 * indicator->LastError + indicator->PrevError);
    double Increment = Proportion + Intergral + Differential;

    float Output = 0;
    Output = Increment > indicator->OutputMax ? indicator->OutputMax : Increment;
    Output = Increment < -indicator->OutputMax ? -indicator->OutputMax : Increment;

    return Output;
}

double PID_AlgorithmCall(PID_Indicator* indicator)
{
    double Increment = PID_Calculate(indicator);
    indicator->ActiveValue = Increment + indicator->ActiveValue;
    printf("Error = %.4f \tIncrement = %.4f \n", indicator->CurrError, Increment);
    return Increment;
}

int main() {

    PID_Indicator indicator = PID_Initial();
    PID_SetParameters(&indicator, 150, 0, 200);
    int cout = 0;

    while (1) {
        printf("Iteration %d:", cout);
        float Output = PID_AlgorithmCall(&indicator);
        cout++;
        if (Output == -0.0000)
            break;
    }
    return 0;
}
